/**
 *******************************************************************************
 * @file      : lqr.cpp
 * @brief     :
 * @history   :
 *  Version     Date            Author          Note
 *  V0.9.0      yyyy-mm-dd      <author>        1. <note>
 *******************************************************************************
 * @attention :
 *******************************************************************************
 *  Copyright (c) 2024 Reborn Team, USTB.
 *  All Rights Reserved.
 *******************************************************************************
 */
/* Includes ------------------------------------------------------------------*/
#include "lqr.h"

#include "stdint.h"
/* Private macro -------------------------------------------------------------*/
/* Private constants ---------------------------------------------------------*/
/* Private types -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
const float k[12][3] = {
    175.9120,  -170.6678, -17.1254, 35.0040,  -31.1551, -3.6713,
    20.3988,   -21.8621,  -6.6372,  16.7956,  -19.7366, -10.9749,
    101.1676,  -109.3629, 43.8122,  18.3027,  -19.2733, 8.2075,
    -161.4978, 56.6788,   27.0371,  -1.5529,  -14.1453, 11.7354,
    -36.9927,  11.8450,   6.5878,   -31.0895, 2.3105,   10.9002,
    -188.9519, 156.6850,  49.1255,  -31.0208, 26.7038,  7.4055,
};
/* External variables --------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/

void Lqr::Calc() {
  float lsqr = leg_len_ * leg_len_;

  for (uint8_t i = 0; i < 2; ++i) {
    uint8_t j = i * 6;
    T_K_[i][0] =
        (k[j + 0][0] * lsqr + k[j + 0][1] * leg_len_ + k[j + 0][2]) * -theta_;
    T_K_[i][1] =
        (k[j + 1][0] * lsqr + k[j + 1][1] * leg_len_ + k[j + 1][2]) * -w_theta_;
    T_K_[i][2] =
        (k[j + 2][0] * lsqr + k[j + 2][1] * leg_len_ + k[j + 2][2]) * -dist_;
    T_K_[i][3] = (k[j + 3][0] * lsqr + k[j + 3][1] * leg_len_ + k[j + 3][2]) *
                 (target_speed_ - speed_);
    T_K_[i][4] =
        (k[j + 4][0] * lsqr + k[j + 4][1] * leg_len_ + k[j + 4][2]) * -phi_;
    T_K_[i][5] =
        (k[j + 5][0] * lsqr + k[j + 5][1] * leg_len_ + k[j + 5][2]) * -w_phi_;
  }

  if (F_N_ < 20.0f) {
    for (uint8_t i = 0; i < 6; ++i) {
      T_K_[0][i] = 0.0f;
    };
    T_K_[1][2] = T_K_[1][3] = T_K_[1][4] = T_K_[1][5] = 0.0f;
  }

  for (uint8_t i = 0; i < 2; ++i) {
    T_[i] = T_K_[i][0] + T_K_[i][1] + T_K_[i][2] + T_K_[i][3] + T_K_[i][4] +
            T_K_[i][5];
  }
}